\documentclass[]{article}

\usepackage{amsmath, amssymb, graphics, setspace}
\usepackage{graphicx}
\usepackage{float}
\newcommand{\mathsym}[1]{{}}
\newcommand{\unicode}[1]{{}}

\newcounter{mathematicapage}

%opening
\title{Optimal Control - Homework Excerise 3}
\author{  Henrik Edlund, 900202-4736, hened061\\ 
Johan Andersson 900612-0332, johan474}

\begin{document}

\maketitle
\section{The Zermelo Problem}
\subsection{Analytic solution} 
\subsubsection*{a)}
The Hameltonian H will be defined as follows,

\begin{equation*}
H = f_0 + \lambda^Tf = 0 + \lambda^T
 \begin{bmatrix}
\omega cos(\theta(t)) +y(t)    \\[0.3em]
\omega sin(\theta (t))              \\[0.3em]
\end{bmatrix}
\end{equation*}

and the adjoint equations that will be solved looks like this,

\begin{equation*}
\dot\lambda_i = - \frac{\partial H}{\partial x_i}
\Leftrightarrow
\end{equation*}

\begin{equation*}
 \begin{bmatrix}
\dot \lambda_1    \\[0.3em]
\dot \lambda_2            \\[0.3em]
\end{bmatrix}
 = -
 \begin{bmatrix}
\frac{\partial H}{\partial x} ( \lambda_1[\omega cos(\theta(t)) +y(t)] +  \lambda_2[\omega sin(\theta (t))] ) \\[0.3em]
\frac{\partial H}{\partial y} (\lambda_1[\omega cos(\theta(t)) +y(t)] +  \lambda_2[\omega sin(\theta (t))]   )           \\[0.3em]
\end{bmatrix}
=
 \begin{bmatrix}
0   \\[0.3em]
- \lambda_1    \\[0.3em]
\end{bmatrix}
\end{equation*}

\begin{equation*}
 \begin{bmatrix}
\lambda_1(t_f)    \\[0.3em]
\lambda_2(t_f)   \\[0.3em]
\end{bmatrix}
=
 \begin{bmatrix}
 \frac{\partial \Phi}{\partial x}    \\[0.3em]
 \frac{\partial \Phi}{\partial y}  \\[0.3em]
\end{bmatrix}
=
 \begin{bmatrix}
-1    \\[0.3em]
 0  \\[0.3em]
\end{bmatrix}
\Rightarrow
\end{equation*}

\begin{equation*}
 \begin{bmatrix}
\dot \lambda_1    \\[0.3em]
\dot \lambda_2            \\[0.3em]
\end{bmatrix}
= 
 \begin{bmatrix}
0 & 0    \\[0.3em]
-1 & 0  \\[0.3em]
\end{bmatrix}
 \begin{bmatrix}
\lambda_1(t_f)    \\[0.3em]
\lambda_2(t_f)   \\[0.3em]
\end{bmatrix}
=
 \begin{bmatrix}
0    \\[0.3em]
1  \\[0.3em]
\end{bmatrix}
\end{equation*}
Which is the result because $\lambda_1$ has no dynamics and because the dynamics of $\lambda_2$ only depend on $\lambda_1$ the change rate will be constant. This means that
\begin{equation*}
 \begin{bmatrix}
\lambda_1(t)    \\[0.3em]
\lambda_2(t)   \\[0.3em]
\end{bmatrix}
=
 \begin{bmatrix}
-1    \\
-t_f + t \\ 
\end{bmatrix}
\end{equation*}

\subsubsection*{b)}
The partial derivative of H w.r.t $\omega$ will be calculated like this:

\begin{equation*}
H = f_0 + \lambda^Tf = 0 + \lambda^T f
= \lambda_1[\omega cos(\theta(t)) +y(t)] +
\lambda_2[\omega sin(\theta (t))]
\end{equation*}
\begin{equation*}
\frac{\partial H}{\partial \theta}
= 
- \lambda_1 \omega sin(\theta) + \lambda_2 \omega cos(\theta) 
= 0
\Leftrightarrow
\end{equation*}
\begin{equation*}
 \lambda_1 \omega sin(\theta) = \lambda_2 \omega cos(\theta)
 \Leftrightarrow
\theta =  \arctan(\frac{\lambda_2}{ \lambda_1})
\end{equation*}

\subsubsection*{c)}
With the results from a) and b) it's easy to see that 
\begin{equation*}
\theta =  \arctan(\frac{\lambda_2}{ \lambda_1})
= \arctan(\frac{-t_f + t}{-1}) = \arctan(t_f -t)
\end{equation*}
Which means that the optimal solution
\begin{equation*}
\theta^* = \arctan(t_f -t)
\end{equation*}
\subsubsection*{d)}
After a simulation of the system, with optimal control signal, in Matlab the result were plotted, which can be seen in figure 1 below. The numerical value of the cost was 1.148.

\begin{figure}[H]

  \centering
    \includegraphics[width=1.0\textwidth]{11d}
      \caption{A simulation of the theoretical solution.}
\end{figure}






\subsection{Discretezation method solution} 
\subsubsection*{a)}
By using the euler approximation,
\begin{align*}
x[k+1]=x[k]+hf(x[k],u[k])
\end{align*}
The following discrete model is obtained,
\begin{align*}
\begin{bmatrix}
x[k+1] \\
y[k+1] 
\end{bmatrix}
=
\begin{bmatrix}
x[k] \\
y[k] 
\end{bmatrix}
+
\begin{bmatrix}
h\cos(\theta)+hy[k] \\
h\sin(\theta) 
\end{bmatrix}
=
\begin{bmatrix}
x[k]+h\cos(\theta)+hy[k] \\
y[k]+h\sin(\theta) 
\end{bmatrix}
\end{align*}
Where, 
\begin{align*}
w=1, \quad v(y)=y
\end{align*}
\subsubsection*{b)}
The resulting states and control signal can be seen in figure \ref{12d}
\begin{figure}[H]
\centering
\includegraphics[width=1.0\textwidth]{12b}
\caption{The states and control signal solved with \textbf{fmincon} with GradObj and GradConstr set to 'on'.}
\label{12d}
\end{figure}

\subsubsection*{c)}
The solution by discretization is very similiar to the analytical solution.
The numerical value of the the cost is 1.143675 .
\subsubsection*{d)}
In this case it does not make a difference because ??










\subsection{Gradient method} 
\subsubsection*{a)}
The gradient method's solution and covnergence rate is very dependent on steplength. Different step lengths can give quite large difference is solutions. A step length which gave a similiar solution as the analytical was used in figure \ref{13a}.
\begin{figure}[H]
\centering
\includegraphics[width=1.0\textwidth]{13a}
\caption{The states and control signal solved with Gradient method, step length 0.5.}
\label{13a}
\end{figure}

\subsubsection*{b)}
The numerical value of the cost is 1.147796.
All three solutions are very similar for certain step lengths for the gradient method.
The prefered method is the discretization method because it doesn't depend on step length to get a good solution.


\section{Max radius orbit transfer in a given time}
\subsection{Boundary condition iteration (Shooting)} 
\subsubsection*{a)}
The Hameltonian in this optimal control problem will look like this
\begin{equation*}
H = \lambda^T f
\end{equation*}

where $\lambda$ will be defined in this way,
\begin{equation*}
\lambda =  (\lambda_r \lambda_u \lambda_v)^T
\end{equation*}
and
\begin{equation*}
f = 
 \begin{bmatrix}
u    \\[0.3em]
\frac{v^2}{r} - \frac{1}{r^2} + asin(\theta)  \\[0.3em]
-\frac{uv}{r}+ acos(\theta)    \\[0.3em]
\end{bmatrix}
\end{equation*}

\begin{equation*}
\Rightarrow
H = \lambda_r u + \lambda_u(\frac{v^2}{r} -\frac{1}{r^2} + asin(\theta) ) + \lambda_v(-\frac{uv}{r}+acos(\theta))
\end{equation*}

The adjoint equation that needs to be solved,
\begin{equation*}
 \begin{bmatrix}
\dot \lambda_r    \\[0.3em]
\dot \lambda_u            \\[0.3em]
\dot \lambda_v \\
\end{bmatrix}
 = H_x = 
  \begin{bmatrix}
 \lambda_u(-\frac{v^2}{r^2}+\frac{2}{r^3}) +\lambda_v \frac{uv}{r^2}    \\[0.3em]
 \lambda_r - \lambda_v \frac{v}{r}            \\[0.3em]
 \lambda_u \frac{2v}{r} -\lambda_v \frac{u}{r} \\
 \end{bmatrix}
\end{equation*}

\begin{equation*}
 \begin{bmatrix}
\lambda_r(t_f)    \\[0.3em]
\lambda_u(t_f)   \\[0.3em]
\lambda_v(t_f)   \\[0.3em]
\end{bmatrix}
=
 \frac{\partial \Phi(t_f)}{\partial x}
 = 
  \begin{bmatrix}
-r(t_f)    \\[0.3em]
0  \\[0.3em]
0   \\[0.3em]
 \end{bmatrix}
\end{equation*}


\subsubsection*{b)}
The terminal constraint of the adjoint equations are given by the following expression,
\begin{equation*}
\lambda(t_f) = \lambda_0 \nabla \Phi(x(t_f)) + G_x(x(t_f))^T \nu
\end{equation*}
where
\begin{equation*}
\nu \in \mathbb{R}^2 \quad and \quad \lambda_0 = 1
\end{equation*}




\subsubsection*{c)}
The optimal control signal can be calculated using by calculating $H_\theta = 0$. Which gives the following equations,
\begin{equation*}
H_\theta = \lambda_u a cos(\theta) - \lambda_v a sin(\theta)
\Leftrightarrow
\end{equation*}
\begin{equation*}
 \lambda_u a cos(\theta) = \lambda_v a sin(\theta)
 \Leftrightarrow
\end{equation*}
\begin{equation*}
 \tan (\theta) = \frac{\lambda_u}{\lambda_v}
\end{equation*}
\subsubsection*{d)}
The numerical value of the optimal cost in this problem when using \\ $\lambda(0)$ = (-1  -1  -1)  is  1.523056. If a bad $\lambda(0)$ is used the solution won't converge. The states and control signals can be seen in figure \ref{21d} and the trajectory can be seen in figure \ref{21dtraj}
\begin{figure}[H]
\centering
\includegraphics[width=1.0\textwidth]{21d}
\caption{The states and control signal solved with Shooting method}
\label{21d}
\end{figure}
\begin{figure}[H]
\centering
\includegraphics[width=1.0\textwidth]{21dtraj}
\caption{The trajectory of the vessel}
\label{21dtraj}
\end{figure}

\subsection{PROPT} 
\subsubsection*{a)}
The value of the optimal cost is 1.523666.
The states and control signal of the optimal solution can be seen in figure \ref{22a}
\begin{figure}[H]
\centering
\includegraphics[width=1.0\textwidth]{22a}
\caption{The states and control signal solved with PROPT}
\label{22a}
\end{figure}
\subsubsection*{b)}
The problem is solved twice, once with lesser solution points to get a rough estimated of the solution. The second time the rough solution is used as initual guess as the problem is solved with many solution points. For this particular problem it doesn't seem necessery to solve it twice. The solution with 20 and then 100 points give the same solution as using 100 points directly.




\end{document}
